/*
 * File: Ctrl.cpp
 *
 * Code generated for Simulink model 'Ctrl'.
 *
 * Model version                  : 1.107
 * Simulink Coder version         : 8.2 (R2012a) 29-Dec-2011
 * TLC version                    : 8.2 (Dec 29 2011)
 * C/C++ source code generated on : Sun Apr 29 22:50:37 2012
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: Texas Instruments->C2000
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "Ctrl.h"
#include "Ctrl_private.h"

/* Named constants for Chart: '<S1>/Chart' */
#define Ctrl_IN_Failed                 ((uint8_T)1U)
#define Ctrl_IN_NO_ACTIVE_CHILD        ((uint8_T)0U)
#define Ctrl_IN_Off                    ((uint8_T)1U)
#define Ctrl_IN_Offline                ((uint8_T)2U)
#define Ctrl_IN_OmegaMode              ((uint8_T)2U)
#define Ctrl_IN_Online                 ((uint8_T)3U)
#define Ctrl_IN_OverCurrentTest        ((uint8_T)1U)
#define Ctrl_IN_PWMMode                ((uint8_T)3U)
#define Ctrl_IN_Powerup                ((uint8_T)4U)
#define Ctrl_IN_YawMode                ((uint8_T)4U)
#define Ctrl_event_FAIL                (0L)

/* Function for Chart: '<S1>/Chart' */
void CtrlModelClass::Ctrl_SetOutputs(real_T state)
{
  /* Graphical Function 'SetOutputs': '<S3>:21' */
  /* Transition: '<S3>:35' */
  /* Transition: '<S3>:36' */
  /*  All Off */
  Ctrl_B.CtrlOff = FALSE;
  Ctrl_B.CtrlPWM = FALSE;
  Ctrl_B.CtrlOmega = FALSE;
  Ctrl_B.CtrlYaw = FALSE;
  if (state == 0.0) {
    /* Transition: '<S3>:38' */
    /* Transition: '<S3>:43' */
    /*  Off */
    Ctrl_B.CtrlOff = TRUE;

    /* Transition: '<S3>:47' */
  } else if (state == 2.0) {
    /* Transition: '<S3>:96' */
    /* Transition: '<S3>:93' */
    /*  OmegaMode */
    Ctrl_B.CtrlOmega = TRUE;

    /* Transition: '<S3>:95' */
  } else if (state == 1.0) {
    /* Transition: '<S3>:40' */
    /* Transition: '<S3>:42' */
    /*  PWMMode */
    Ctrl_B.CtrlPWM = TRUE;

    /* Transition: '<S3>:49' */
  } else {
    if (state == 3.0) {
      /* Transition: '<S3>:105' */
      /* Transition: '<S3>:108' */
      /*  YawMode */
      Ctrl_B.CtrlYaw = TRUE;

      /* Transition: '<S3>:109' */
    }
  }
}

/* Function for Chart: '<S1>/Chart' */
void CtrlModelClass::Ctrl_SystemLogic(void)
{
  /* During 'SystemLogic': '<S3>:12' */
  switch (Ctrl_DWork.is_SystemLogic) {
   case Ctrl_IN_Failed:
    /* During 'Failed': '<S3>:3' */
    if (Ctrl_U.AckErrr) {
      /* Transition: '<S3>:11' */
      Ctrl_DWork.is_SystemLogic = Ctrl_IN_Offline;

      /* Entry 'Offline': '<S3>:1' */
      Ctrl_B.AState = 0UL;
      Ctrl_B.Kill = TRUE;
    }
    break;

   case Ctrl_IN_Offline:
    /* During 'Offline': '<S3>:1' */
    if (Ctrl_U.Active) {
      /* Transition: '<S3>:4' */
      Ctrl_DWork.is_SystemLogic = Ctrl_IN_Powerup;
      Ctrl_DWork.temporalCounter_i1 = 0U;

      /* Entry 'Powerup': '<S3>:5' */
      Ctrl_B.Kill = FALSE;
    }
    break;

   case Ctrl_IN_Online:
    /* During 'Online': '<S3>:2' */
    if (!Ctrl_U.Active) {
      /* Transition: '<S3>:7' */
      /* Exit Internal 'Online': '<S3>:2' */
      Ctrl_DWork.is_Online = Ctrl_IN_NO_ACTIVE_CHILD;
      if ((int16_T)Ctrl_DWork.is_SystemLogic == (int16_T)Ctrl_IN_Online) {
        /* Exit 'Online': '<S3>:2' */
        Ctrl_B.AMode = 0UL;
        Ctrl_SetOutputs(0.0);
        Ctrl_DWork.is_SystemLogic = Ctrl_IN_NO_ACTIVE_CHILD;
      }

      if ((int16_T)Ctrl_DWork.is_SystemLogic != (int16_T)Ctrl_IN_Offline) {
        Ctrl_DWork.is_SystemLogic = Ctrl_IN_Offline;

        /* Entry 'Offline': '<S3>:1' */
        Ctrl_B.AState = 0UL;
        Ctrl_B.Kill = TRUE;
      }
    } else if (Ctrl_DWork.sfEvent == Ctrl_event_FAIL) {
      /* Transition: '<S3>:10' */
      /* Exit Internal 'Online': '<S3>:2' */
      Ctrl_DWork.is_Online = Ctrl_IN_NO_ACTIVE_CHILD;
      if ((int16_T)Ctrl_DWork.is_SystemLogic == (int16_T)Ctrl_IN_Online) {
        /* Exit 'Online': '<S3>:2' */
        Ctrl_B.AMode = 0UL;
        Ctrl_SetOutputs(0.0);
        Ctrl_DWork.is_SystemLogic = Ctrl_IN_NO_ACTIVE_CHILD;
      }

      if ((int16_T)Ctrl_DWork.is_SystemLogic != (int16_T)Ctrl_IN_Failed) {
        Ctrl_DWork.is_SystemLogic = Ctrl_IN_Failed;

        /* Entry 'Failed': '<S3>:3' */
        Ctrl_B.AState = 2UL;
        Ctrl_B.Kill = TRUE;
      }
    } else if ((Ctrl_U.Mode == 0UL) && (!((int16_T)Ctrl_DWork.is_Online ==
                 (int16_T)Ctrl_IN_Off))) {
      /* Transition: '<S3>:15' */
      /* Exit Internal 'Online': '<S3>:2' */
      Ctrl_DWork.is_Online = Ctrl_IN_NO_ACTIVE_CHILD;
      if ((int16_T)Ctrl_DWork.is_Online != (int16_T)Ctrl_IN_Off) {
        Ctrl_DWork.is_Online = Ctrl_IN_Off;

        /* Entry 'Off': '<S3>:14' */
        Ctrl_B.Kill = FALSE;
        Ctrl_B.AMode = 0UL;
        Ctrl_SetOutputs(0.0);
      }
    } else {
      switch (Ctrl_DWork.is_Online) {
       case Ctrl_IN_Off:
        /* During 'Off': '<S3>:14' */
        if (Ctrl_U.Mode == 1UL) {
          /* Transition: '<S3>:17' */
          Ctrl_DWork.is_Online = Ctrl_IN_PWMMode;

          /* Entry 'PWMMode': '<S3>:13' */
          Ctrl_B.AMode = 1UL;
          Ctrl_SetOutputs((real_T)Ctrl_B.AMode);
        } else if (Ctrl_U.Mode == 2UL) {
          /* Transition: '<S3>:89' */
          Ctrl_DWork.is_Online = Ctrl_IN_OmegaMode;

          /* Entry 'OmegaMode': '<S3>:87' */
          Ctrl_B.AMode = 2UL;
          Ctrl_SetOutputs((real_T)Ctrl_B.AMode);
        } else {
          if (Ctrl_U.Mode == 3UL) {
            /* Transition: '<S3>:90' */
            Ctrl_DWork.is_Online = Ctrl_IN_YawMode;

            /* Entry 'YawMode': '<S3>:88' */
            Ctrl_B.AMode = 3UL;
            Ctrl_SetOutputs((real_T)Ctrl_B.AMode);
          }
        }
        break;

       case Ctrl_IN_OmegaMode:
        break;

       case Ctrl_IN_PWMMode:
        break;

       case Ctrl_IN_YawMode:
        break;

       default:
        Ctrl_DWork.is_Online = Ctrl_IN_NO_ACTIVE_CHILD;
        break;
      }
    }
    break;

   case Ctrl_IN_Powerup:
    /* During 'Powerup': '<S3>:5' */
    if (Ctrl_DWork.temporalCounter_i1 >= 50U) {
      /* Transition: '<S3>:6' */
      Ctrl_DWork.is_SystemLogic = Ctrl_IN_Online;

      /* Entry 'Online': '<S3>:2' */
      Ctrl_B.AState = 1UL;

      /* Entry Internal 'Online': '<S3>:2' */
      /* Transition: '<S3>:16' */
      if ((int16_T)Ctrl_DWork.is_Online != (int16_T)Ctrl_IN_Off) {
        Ctrl_DWork.is_Online = Ctrl_IN_Off;

        /* Entry 'Off': '<S3>:14' */
        Ctrl_B.Kill = FALSE;
        Ctrl_B.AMode = 0UL;
        Ctrl_SetOutputs(0.0);
      }
    }
    break;

   default:
    Ctrl_DWork.is_SystemLogic = Ctrl_IN_NO_ACTIVE_CHILD;
    break;
  }
}

/* Function for Chart: '<S1>/Chart' */
void CtrlModelClass::Ctrl_TestOverCurrent(void)
{
  int32_T b_previousEvent;

  /* Graphical Function 'TestOverCurrent': '<S3>:61' */
  /* Transition: '<S3>:65' */
  if ((real32_T)fabs(Ctrl_U.Curr1) > 30.0F) {
    /* Transition: '<S3>:67' */
    /* Transition: '<S3>:71' */
    b_previousEvent = Ctrl_DWork.sfEvent;
    Ctrl_DWork.sfEvent = Ctrl_event_FAIL;
    if ((int16_T)Ctrl_DWork.is_active_SystemLogic != 0) {
      Ctrl_SystemLogic();
    }

    Ctrl_DWork.sfEvent = b_previousEvent;

    /* Transition: '<S3>:72' */
  } else {
    /* Transition: '<S3>:73' */
  }

  /* Transition: '<S3>:75' */
}

/* Model step function */
void CtrlModelClass::step()
{
  real32_T rtb_y;
  real32_T rtb_IntegralGain;
  real32_T rtb_Sum_o;
  real32_T rtb_Saturation_p;
  real32_T rtb_Sum_o_0;
  real32_T rtb_IntegralGain_0;

  /* Outputs for Atomic SubSystem: '<Root>/Ctrl' */
  /* MATLAB Function: '<S2>/MATLAB Function' incorporates:
   *  Bias: '<S2>/Bias'
   *  Bias: '<S2>/Bias1'
   *  Inport: '<Root>/MagY'
   *  Inport: '<Root>/MagZ'
   *  Trigonometry: '<S2>/Trigonometric Function'
   */
  /* MATLAB Function 'Ctrl/Angleconverter/MATLAB Function': '<S8>:1' */
  /* '<S8>:1:3' */
  rtb_y = (real32_T)atan2(Ctrl_U.MagY + Ctrl_P.Bias_Bias, Ctrl_U.MagZ +
    Ctrl_P.Bias1_Bias) / 6.28318548F;
  rtb_y = (rtb_y - (real32_T)floor(rtb_y)) * 6.28318548F;

  /* Chart: '<S1>/Chart' */
  Ctrl_DWork.presentTicks = Ctrl_M->Timing.clockTick0;
  Ctrl_DWork.elapsedTicks = Ctrl_DWork.presentTicks - Ctrl_DWork.previousTicks;
  Ctrl_DWork.previousTicks = Ctrl_DWork.presentTicks;
  if ((uint32_T)Ctrl_DWork.temporalCounter_i1 + Ctrl_DWork.elapsedTicks <= 63UL)
  {
    Ctrl_DWork.temporalCounter_i1 = (uint16_T)((uint32_T)
      Ctrl_DWork.temporalCounter_i1 + Ctrl_DWork.elapsedTicks) & 255U;
  } else {
    Ctrl_DWork.temporalCounter_i1 = 63U;
  }

  /* Gateway: Ctrl/Chart */
  Ctrl_DWork.sfEvent = -1L;

  /* During: Ctrl/Chart */
  if ((int16_T)Ctrl_DWork.is_active_c1_Ctrl == 0) {
    /* Entry: Ctrl/Chart */
    Ctrl_DWork.is_active_c1_Ctrl = 1U;

    /* Entry Internal: Ctrl/Chart */
    Ctrl_DWork.is_active_SystemLogic = 1U;

    /* Entry Internal 'SystemLogic': '<S3>:12' */
    /* Transition: '<S3>:86' */
    if ((int16_T)Ctrl_DWork.is_SystemLogic != (int16_T)Ctrl_IN_Offline) {
      Ctrl_DWork.is_SystemLogic = Ctrl_IN_Offline;

      /* Entry 'Offline': '<S3>:1' */
      Ctrl_B.AState = 0UL;
      Ctrl_B.Kill = TRUE;
    }

    Ctrl_DWork.is_active_FailureDetection = 1U;

    /* Entry Internal 'FailureDetection': '<S3>:53' */
    Ctrl_DWork.is_FailureDetection = Ctrl_IN_OverCurrentTest;
  } else {
    if ((int16_T)Ctrl_DWork.is_active_SystemLogic != 0) {
      Ctrl_SystemLogic();
    }

    if (!(((int16_T)Ctrl_DWork.is_active_FailureDetection == 0) || ((int16_T)
          Ctrl_DWork.is_FailureDetection != (int16_T)Ctrl_IN_OverCurrentTest)))
    {
      /* During 'FailureDetection': '<S3>:53' */
      /* During 'OverCurrentTest': '<S3>:54' */
      Ctrl_TestOverCurrent();
    }
  }

  /* End of Chart: '<S1>/Chart' */

  /* Outputs for Enabled SubSystem: '<S1>/GyroMode' incorporates:
   *  EnablePort: '<S4>/Enable'
   */
  if (Ctrl_B.CtrlOmega) {
    if (!Ctrl_DWork.GyroMode_MODE) {
      /* InitializeConditions for DiscreteIntegrator: '<S9>/Integrator' */
      Ctrl_DWork.Integrator_DSTATE_j = Ctrl_P.Integrator_IC;
      Ctrl_DWork.GyroMode_MODE = TRUE;
    }

    /* Sum: '<S4>/Sum2' incorporates:
     *  Inport: '<Root>/OmegaRef'
     *  Inport: '<Root>/OmegaZ'
     */
    rtb_IntegralGain = Ctrl_U.OmegaRef - Ctrl_U.OmegaZ;

    /* Sum: '<S9>/Sum' incorporates:
     *  DiscreteIntegrator: '<S9>/Integrator'
     *  Gain: '<S9>/Proportional Gain'
     */
    rtb_Sum_o = Ctrl_P.ProportionalGain_Gain * rtb_IntegralGain +
      Ctrl_DWork.Integrator_DSTATE_j;

    /* Saturate: '<S9>/Saturation' */
    if (rtb_Sum_o >= Ctrl_P.Saturation_UpperSat) {
      rtb_Saturation_p = Ctrl_P.Saturation_UpperSat;
    } else if (rtb_Sum_o <= Ctrl_P.Saturation_LowerSat) {
      rtb_Saturation_p = Ctrl_P.Saturation_LowerSat;
    } else {
      rtb_Saturation_p = rtb_Sum_o;
    }

    /* End of Saturate: '<S9>/Saturation' */

    /* Gain: '<S9>/Integral Gain' */
    rtb_IntegralGain *= Ctrl_P.IntegralGain_Gain;

    /* Sum: '<S4>/Sum' incorporates:
     *  Inport: '<Root>/PWMRef1'
     */
    Ctrl_B.PWM1 = Ctrl_U.PWMRef1 + rtb_Saturation_p;

    /* Sum: '<S4>/Sum1' incorporates:
     *  Inport: '<Root>/PWMRef1'
     */
    Ctrl_B.PWM2 = Ctrl_U.PWMRef1 - rtb_Saturation_p;

    /* Signum: '<S10>/SignPreSat' */
    if (rtb_Sum_o < 0.0F) {
      rtb_Sum_o_0 = -1.0F;
    } else if (rtb_Sum_o > 0.0F) {
      rtb_Sum_o_0 = 1.0F;
    } else {
      rtb_Sum_o_0 = rtb_Sum_o;
    }

    /* Signum: '<S10>/SignPreIntegrator' */
    if (rtb_IntegralGain < 0.0F) {
      rtb_IntegralGain_0 = -1.0F;
    } else if (rtb_IntegralGain > 0.0F) {
      rtb_IntegralGain_0 = 1.0F;
    } else {
      rtb_IntegralGain_0 = rtb_IntegralGain;
    }

    /* Switch: '<S9>/Switch' incorporates:
     *  Constant: '<S9>/Constant'
     *  Logic: '<S10>/AND'
     *  RelationalOperator: '<S10>/Equal'
     *  RelationalOperator: '<S10>/NotEqual'
     *  Signum: '<S10>/SignPreIntegrator'
     *  Signum: '<S10>/SignPreSat'
     */
    if ((rtb_Sum_o != rtb_Saturation_p) && (rtb_Sum_o_0 == rtb_IntegralGain_0))
    {
      rtb_IntegralGain = Ctrl_P.Constant_Value;
    }

    /* End of Switch: '<S9>/Switch' */

    /* Update for DiscreteIntegrator: '<S9>/Integrator' */
    Ctrl_DWork.Integrator_DSTATE_j += Ctrl_P.Integrator_gainval *
      rtb_IntegralGain;
  } else {
    if (Ctrl_DWork.GyroMode_MODE) {
      Ctrl_DWork.GyroMode_MODE = FALSE;
    }
  }

  /* End of Outputs for SubSystem: '<S1>/GyroMode' */

  /* Outputs for Enabled SubSystem: '<S1>/GyroMode1' incorporates:
   *  EnablePort: '<S5>/Enable'
   */
  if (Ctrl_B.CtrlYaw) {
    if (!Ctrl_DWork.GyroMode1_MODE) {
      /* InitializeConditions for DiscreteIntegrator: '<S11>/Integrator' */
      Ctrl_DWork.Integrator_DSTATE = Ctrl_P.Integrator_IC_m;
      Ctrl_DWork.GyroMode1_MODE = TRUE;
    }

    /* Gain: '<S12>/Proportional Gain' incorporates:
     *  Inport: '<Root>/YawRef'
     *  Sum: '<S5>/Sum3'
     */
    rtb_IntegralGain = (Ctrl_U.YawRef - rtb_y) * Ctrl_P.ProportionalGain_Gain_j;

    /* Saturate: '<S12>/Saturation' */
    if (rtb_IntegralGain >= Ctrl_P.Saturation_UpperSat_k) {
      rtb_IntegralGain = Ctrl_P.Saturation_UpperSat_k;
    } else {
      if (rtb_IntegralGain <= Ctrl_P.Saturation_LowerSat_j) {
        rtb_IntegralGain = Ctrl_P.Saturation_LowerSat_j;
      }
    }

    /* Sum: '<S5>/Sum2' incorporates:
     *  Inport: '<Root>/OmegaZ'
     *  Saturate: '<S12>/Saturation'
     */
    rtb_IntegralGain -= Ctrl_U.OmegaZ;

    /* Sum: '<S11>/Sum' incorporates:
     *  DiscreteIntegrator: '<S11>/Integrator'
     *  Gain: '<S11>/Proportional Gain'
     */
    rtb_Sum_o = Ctrl_P.ProportionalGain_Gain_h * rtb_IntegralGain +
      Ctrl_DWork.Integrator_DSTATE;

    /* Saturate: '<S11>/Saturation' */
    if (rtb_Sum_o >= Ctrl_P.Saturation_UpperSat_h) {
      rtb_Saturation_p = Ctrl_P.Saturation_UpperSat_h;
    } else if (rtb_Sum_o <= Ctrl_P.Saturation_LowerSat_i) {
      rtb_Saturation_p = Ctrl_P.Saturation_LowerSat_i;
    } else {
      rtb_Saturation_p = rtb_Sum_o;
    }

    /* End of Saturate: '<S11>/Saturation' */

    /* Gain: '<S11>/Integral Gain' */
    rtb_IntegralGain *= Ctrl_P.IntegralGain_Gain_d;

    /* Sum: '<S5>/Sum' incorporates:
     *  Inport: '<Root>/PWMRef1'
     */
    Ctrl_B.PWM1 = Ctrl_U.PWMRef1 + rtb_Saturation_p;

    /* Sum: '<S5>/Sum1' incorporates:
     *  Inport: '<Root>/PWMRef1'
     */
    Ctrl_B.PWM2 = Ctrl_U.PWMRef1 - rtb_Saturation_p;

    /* Signum: '<S13>/SignPreSat' */
    if (rtb_Sum_o < 0.0F) {
      rtb_Sum_o_0 = -1.0F;
    } else if (rtb_Sum_o > 0.0F) {
      rtb_Sum_o_0 = 1.0F;
    } else {
      rtb_Sum_o_0 = rtb_Sum_o;
    }

    /* Signum: '<S13>/SignPreIntegrator' */
    if (rtb_IntegralGain < 0.0F) {
      rtb_IntegralGain_0 = -1.0F;
    } else if (rtb_IntegralGain > 0.0F) {
      rtb_IntegralGain_0 = 1.0F;
    } else {
      rtb_IntegralGain_0 = rtb_IntegralGain;
    }

    /* Switch: '<S11>/Switch' incorporates:
     *  Constant: '<S11>/Constant'
     *  Logic: '<S13>/AND'
     *  RelationalOperator: '<S13>/Equal'
     *  RelationalOperator: '<S13>/NotEqual'
     *  Signum: '<S13>/SignPreIntegrator'
     *  Signum: '<S13>/SignPreSat'
     */
    if ((rtb_Sum_o != rtb_Saturation_p) && (rtb_Sum_o_0 == rtb_IntegralGain_0))
    {
      rtb_IntegralGain = Ctrl_P.Constant_Value_g;
    }

    /* End of Switch: '<S11>/Switch' */

    /* Update for DiscreteIntegrator: '<S11>/Integrator' */
    Ctrl_DWork.Integrator_DSTATE += Ctrl_P.Integrator_gainval_f *
      rtb_IntegralGain;
  } else {
    if (Ctrl_DWork.GyroMode1_MODE) {
      Ctrl_DWork.GyroMode1_MODE = FALSE;
    }
  }

  /* End of Outputs for SubSystem: '<S1>/GyroMode1' */

  /* Outputs for Enabled SubSystem: '<S1>/Off' incorporates:
   *  EnablePort: '<S6>/Enable'
   */
  if (Ctrl_B.CtrlOff) {
    /* Constant: '<S6>/Constant1' */
    Ctrl_B.PWM2 = Ctrl_P.Constant1_Value;

    /* Constant: '<S6>/Constant2' */
    Ctrl_B.PWM1 = Ctrl_P.Constant2_Value;
  }

  /* End of Outputs for SubSystem: '<S1>/Off' */

  /* Outputs for Enabled SubSystem: '<S1>/PWMMode' incorporates:
   *  EnablePort: '<S7>/Enable'
   */
  if (Ctrl_B.CtrlPWM) {
    /* Inport: '<S7>/PWMIn1' incorporates:
     *  Inport: '<Root>/PWMRef1'
     */
    Ctrl_B.PWM1 = Ctrl_U.PWMRef1;

    /* Inport: '<S7>/PWMIn2' incorporates:
     *  Inport: '<Root>/PWMRef2'
     */
    Ctrl_B.PWM2 = Ctrl_U.PWMRef2;
  }

  /* End of Outputs for SubSystem: '<S1>/PWMMode' */
  /* End of Outputs for SubSystem: '<Root>/Ctrl' */

  /* Outport: '<Root>/Kill' */
  Ctrl_Y.Kill = Ctrl_B.Kill;

  /* Outport: '<Root>/ActualState' */
  Ctrl_Y.ActualState = Ctrl_B.AState;

  /* Outport: '<Root>/ActualMode' */
  Ctrl_Y.ActualMode = Ctrl_B.AMode;

  /* Outport: '<Root>/PWM1' */
  Ctrl_Y.PWM1 = Ctrl_B.PWM1;

  /* Outport: '<Root>/PWM2' */
  Ctrl_Y.PWM2 = Ctrl_B.PWM2;

  /* Outport: '<Root>/YawRad' */
  Ctrl_Y.YawRad = rtb_y;

  /* Update absolute time for base rate */
  /* The "clockTick0" counts the number of times the code of this task has
   * been executed. The resolution of this integer timer is 0.01, which is the step size
   * of the task. Size of "clockTick0" ensures timer will not overflow during the
   * application lifespan selected.
   * Timer of this task consists of two 32 bit unsigned integers.
   * The two integers represent the low bits Timing.clockTick0 and the high bits
   * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
   */
  Ctrl_M->Timing.clockTick0++;
  if (!Ctrl_M->Timing.clockTick0) {
    Ctrl_M->Timing.clockTickH0++;
  }
}

/* Model initialize function */
void CtrlModelClass::initialize()
{
  /* Registration code */

  /* initialize real-time model */
  (void) memset((void *)Ctrl_M, 0,
                sizeof(RT_MODEL_Ctrl));

  /* block I/O */
  (void) memset(((void *) &Ctrl_B), 0,
                sizeof(BlockIO_Ctrl));

  /* states (dwork) */
  (void) memset((void *)&Ctrl_DWork, 0,
                sizeof(D_Work_Ctrl));

  /* external inputs */
  (void) memset((void *)&Ctrl_U, 0,
                sizeof(ExternalInputs_Ctrl));

  /* external outputs */
  (void) memset((void *)&Ctrl_Y, 0,
                sizeof(ExternalOutputs_Ctrl));

  /* Start for Atomic SubSystem: '<Root>/Ctrl' */
  /* Start for Enabled SubSystem: '<S1>/GyroMode' */
  /* InitializeConditions for DiscreteIntegrator: '<S9>/Integrator' */
  Ctrl_DWork.Integrator_DSTATE_j = Ctrl_P.Integrator_IC;

  /* End of Start for SubSystem: '<S1>/GyroMode' */

  /* Start for Enabled SubSystem: '<S1>/GyroMode1' */
  /* InitializeConditions for DiscreteIntegrator: '<S11>/Integrator' */
  Ctrl_DWork.Integrator_DSTATE = Ctrl_P.Integrator_IC_m;

  /* End of Start for SubSystem: '<S1>/GyroMode1' */

  /* Start for Enabled SubSystem: '<S1>/Off' */
  /* VirtualOutportStart for Outport: '<S6>/PWMs' */
  Ctrl_B.PWM1 = Ctrl_P.PWMs_Y0_g[0];
  Ctrl_B.PWM2 = Ctrl_P.PWMs_Y0_g[1];

  /* End of Start for SubSystem: '<S1>/Off' */
  /* End of Start for SubSystem: '<Root>/Ctrl' */

  /* Start for Outport: '<Root>/Kill' */
  Ctrl_Y.Kill = Ctrl_B.Kill;

  /* Start for Outport: '<Root>/ActualState' */
  Ctrl_Y.ActualState = Ctrl_B.AState;

  /* Start for Outport: '<Root>/ActualMode' */
  Ctrl_Y.ActualMode = Ctrl_B.AMode;

  /* Start for Outport: '<Root>/PWM1' */
  Ctrl_Y.PWM1 = Ctrl_B.PWM1;

  /* Start for Outport: '<Root>/PWM2' */
  Ctrl_Y.PWM2 = Ctrl_B.PWM2;

  /* InitializeConditions for Atomic SubSystem: '<Root>/Ctrl' */
  /* InitializeConditions for Chart: '<S1>/Chart' */
  Ctrl_DWork.sfEvent = -1L;
  Ctrl_DWork.is_active_FailureDetection = 0U;
  Ctrl_DWork.is_FailureDetection = 0U;
  Ctrl_DWork.is_active_SystemLogic = 0U;
  Ctrl_DWork.is_SystemLogic = 0U;
  Ctrl_DWork.is_Online = 0U;
  Ctrl_DWork.is_active_c1_Ctrl = 0U;
  Ctrl_B.Kill = FALSE;
  Ctrl_B.AState = 0UL;
  Ctrl_B.AMode = 0UL;
  Ctrl_B.CtrlOff = FALSE;
  Ctrl_B.CtrlPWM = FALSE;
  Ctrl_B.CtrlOmega = FALSE;
  Ctrl_B.CtrlYaw = FALSE;
  Ctrl_DWork.presentTicks = 0UL;
  Ctrl_DWork.elapsedTicks = 0UL;
  Ctrl_DWork.previousTicks = 0UL;

  /* End of InitializeConditions for SubSystem: '<Root>/Ctrl' */

  /* Enable for Atomic SubSystem: '<Root>/Ctrl' */
  /* Enable for Chart: '<S1>/Chart' */
  Ctrl_DWork.presentTicks = Ctrl_M->Timing.clockTick0;
  Ctrl_DWork.previousTicks = Ctrl_DWork.presentTicks;

  /* End of Enable for SubSystem: '<Root>/Ctrl' */
}

/* Model terminate function */
void CtrlModelClass::terminate()
{
  /* (no terminate code required) */
}

/* Constructor */
CtrlModelClass::CtrlModelClass()
{
  Parameters_Ctrl Ctrl_P_temp = {
    /*  Computed Parameter: PWMs_Y0
     * Referenced by: '<S4>/PWMs'
     */
    { 1000.0F, 1000.0F },
    80.0F,                             /* Computed Parameter: ProportionalGain_Gain
                                        * Referenced by: '<S9>/Proportional Gain'
                                        */
    0.01F,                             /* Computed Parameter: Integrator_gainval
                                        * Referenced by: '<S9>/Integrator'
                                        */
    0.0F,                              /* Computed Parameter: Integrator_IC
                                        * Referenced by: '<S9>/Integrator'
                                        */
    200.0F,                            /* Computed Parameter: Saturation_UpperSat
                                        * Referenced by: '<S9>/Saturation'
                                        */
    -200.0F,                           /* Computed Parameter: Saturation_LowerSat
                                        * Referenced by: '<S9>/Saturation'
                                        */
    200.0F,                            /* Computed Parameter: IntegralGain_Gain
                                        * Referenced by: '<S9>/Integral Gain'
                                        */
    0.0F,                              /* Computed Parameter: Constant_Value
                                        * Referenced by: '<S9>/Constant'
                                        */

    /*  Computed Parameter: PWMs_Y0_i
     * Referenced by: '<S5>/PWMs'
     */
    { 1000.0F, 1000.0F },
    3.5F,                             /* Computed Parameter: ProportionalGain_Gain_j
                                        * Referenced by: '<S12>/Proportional Gain'
                                        */
    1.0F,                              /* Computed Parameter: Saturation_UpperSat_k
                                        * Referenced by: '<S12>/Saturation'
                                        */
    -1.0F,                             /* Computed Parameter: Saturation_LowerSat_j
                                        * Referenced by: '<S12>/Saturation'
                                        */
    80.0F,                             /* Computed Parameter: ProportionalGain_Gain_h
                                        * Referenced by: '<S11>/Proportional Gain'
                                        */
    0.01F,                             /* Computed Parameter: Integrator_gainval_f
                                        * Referenced by: '<S11>/Integrator'
                                        */
    0.0F,                              /* Computed Parameter: Integrator_IC_m
                                        * Referenced by: '<S11>/Integrator'
                                        */
    200.0F,                            /* Computed Parameter: Saturation_UpperSat_h
                                        * Referenced by: '<S11>/Saturation'
                                        */
    -200.0F,                           /* Computed Parameter: Saturation_LowerSat_i
                                        * Referenced by: '<S11>/Saturation'
                                        */
    200.0F,                            /* Computed Parameter: IntegralGain_Gain_d
                                        * Referenced by: '<S11>/Integral Gain'
                                        */
    0.0F,                              /* Computed Parameter: Constant_Value_g
                                        * Referenced by: '<S11>/Constant'
                                        */

    /*  Computed Parameter: PWMs_Y0_g
     * Referenced by: '<S6>/PWMs'
     */
    { 1000.0F, 1000.0F },
    1000.0F,                           /* Computed Parameter: Constant1_Value
                                        * Referenced by: '<S6>/Constant1'
                                        */
    1000.0F,                           /* Computed Parameter: Constant2_Value
                                        * Referenced by: '<S6>/Constant2'
                                        */
    -0.0867F,                          /* Computed Parameter: Bias_Bias
                                        * Referenced by: '<S2>/Bias'
                                        */
    0.29F                              /* Computed Parameter: Bias1_Bias
                                        * Referenced by: '<S2>/Bias1'
                                        */
  };                                   /* Modifiable parameters */

  /* Initialize tunable parameters */
  Ctrl_P = Ctrl_P_temp;

  /* Real-Time Model */
  Ctrl_M = &Ctrl_M_;
}

/* Destructor */
CtrlModelClass::~CtrlModelClass()
{
  /* Currently there is no destructor body generated.*/
}

/* Real-Time Model get method */
RT_MODEL_Ctrl * CtrlModelClass::getRTM() const
{
  return Ctrl_M;
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
